// system
#include <string.h>
#include <math.h>
#include <float.h>
#include <stdio.h>
// local
#include "gtrack_int.h"
// log
#include "GtrackPrintf.h"

#ifdef GTRACK_2D
const float pInit[] = { 0.f, 0.f, 0.5f, 0.5f, 1.f, 1.f };
#else
const float pInit[] = { 0.f, 0.f, 0.f, 0.5f, 0.5f, 0.5f, 1.f, 1.f, 1.f };;
#endif // GTRACK_2D

#define MSIZE (GTRACK_MEASUREMENT_VECTOR_SIZE)
#define SSIZE (GTRACK_STATE_VECTOR_SIZE)

#define GTRACK_POS_DIMENSION (0U)
#define GTRACK_VEL_DIMENSION (1U)
#define GTRACK_ACC_DIMENSION (2U)

#define GTRACK_SPREAD_ALPHA                    (0.01f) /* 1 percent */
#define GTRACK_MIN_DISPERSION_ALPHA            (0.1f) /* 10 percent */
#define GTRACK_MIN_POINTS_TO_UPDATE_DISPERSION (2) /* 2 points */
#define GTRACK_MIN_STATIC_VELOCITY             (1.0f) /* 1 m/s 判断静止的条件阈值*/

void* gtrack_unitCreate(TrackingParams* params, int32_t* errCode)
{
    GtrackUnitInstance* inst;

    *errCode = GTRACK_EOK;

    inst = (GtrackUnitInstance*)gtrack_alloc(1, sizeof(GtrackUnitInstance));
    if (inst == NULL)
    {
        *errCode = GTRACK_ENOMEM;
        goto exit;
    }

    memset(inst, 0, sizeof(GtrackUnitInstance));
    /* Parameters */
    inst->gatingParams = &params->gatingParams;
    inst->allocationParams = &params->allocationParams;
    inst->unrollingParams = &params->unrollingParams;
    inst->sceneryParams = &params->sceneryParams;
    inst->radarParams = &params->radarParams;

    memcpy(inst->maxAcceleration, params->maxAcceleration, sizeof(inst->maxAcceleration));

    inst->uid = params->uid;
    inst->isTargetStatic = false;
    inst->initialRadialVelocity = params->initialRadialVelocity;

    inst->F = params->F;
    inst->Q = params->Q;

    switch (params->stateVectorType)
    {

    case GTRACK_STATE_VECTORS_2DA:
        inst->stateVectorType = GTRACK_STATE_VECTORS_2DA;
        inst->stateVectorDimNum = 2;
        inst->stateVectorDimLength = 3;
        inst->stateVectorLength = 6;
        inst->measurementVectorLength = 3;
        break;

    case GTRACK_STATE_VECTORS_3DA:
        inst->stateVectorType = GTRACK_STATE_VECTORS_3DA;
        inst->stateVectorDimNum = 3;
        inst->stateVectorDimLength = 3;
        inst->stateVectorLength = 9;
        inst->measurementVectorLength = 4;
        break;

    default:
        *errCode = GTRACK_EINVAL;
        goto exit;
    }

    inst->state = TRACK_STATE_FREE;

exit:
    if (*errCode != GTRACK_EOK)
    {
        if (inst != NULL)
            gtrack_free(inst, sizeof(GtrackUnitInstance));
        inst = NULL;
    }
    return (void*)inst;
}

void gtrack_unitDelete(void* handle)
{
    gtrack_free(handle, sizeof(GtrackUnitInstance));
}

void gtrack_unitGetH(void* handle, float* pH)
{
    GtrackUnitInstance* inst;

    inst = (GtrackUnitInstance*)handle;
    memcpy(pH, inst->H_s.array, sizeof(inst->H_s.array));
}

void gtrack_unitReport(void* handle, GTRACK_targetDesc* target)
{
    GtrackUnitInstance* inst;

    inst = (GtrackUnitInstance*)handle;
    target->uid = inst->uid;
    target->tid = inst->tid;
    target->state = inst->state;
    target->score = inst->track_state.quality.quality_score;
    memcpy(target->S, inst->S_hat, sizeof(target->S));
    memcpy(target->dim, inst->estDim, sizeof(target->dim));
}

void gtrack_unitStart(void* handle, uint64_t timeStamp, uint32_t tid, GTRACK_measurement_vector* uCenter, float allocSNR)
{
    GtrackUnitInstance* inst;
    GTRACK_measurementUnion u;
    int                     n;

    inst = (GtrackUnitInstance*)handle;

    inst->tid = tid;
    inst->heartBeatCount = timeStamp;
    inst->allocationTime = timeStamp;
    inst->allocationRange = uCenter->range;
    inst->allocationVelocity = uCenter->doppler;
    inst->estNumOfPoints = 0;
    inst->allocSNR = allocSNR;

    // 初始化状态及状态计数
    inst->state = TRACK_STATE_DETECTION;
    inst->active2freeCount = 0;
    inst->detect2activeCount = 0;
    inst->detect2freeCount = 0;
    
    memset(inst->mergedCount, 0, sizeof(inst->mergedCount));

    inst->currentStateVectorType = inst->stateVectorType;
    inst->isTargetStatic = false;

    // 初始化距离、径向速度及径向速度处理类型
    inst->velocityHandling = VELOCITY_INIT;
    u.vector = *uCenter;
    u.vector.doppler = gtrack_unrollRadialVelocity(inst->radarParams->maxRadialVelocity, inst->initialRadialVelocity, uCenter->doppler);
    inst->rangeRate = u.vector.doppler;

    // 初始化先验状态信息
    gtrack_spherical2cartesian(inst->currentStateVectorType, u.array, inst->S_apriori_hat);
    // 初始化Hs矩阵
    memcpy(inst->H_s.array, u.array, sizeof(u.array));
    // 初始化测量误差约束
    gtrack_calcMeasurementLimits(u.vector.range, &inst->gatingParams->limits, &inst->H_limits.vector);

    // 初始化先验过程协方差矩阵
    // P_apriori_hat = diag([0,0,0.5,0.5,1,1])
    memset(inst->P_apriori_hat, 0, sizeof(inst->P_apriori_hat));
    for (n = 0; n < GTRACK_STATE_VECTOR_SIZE; n++)
        inst->P_apriori_hat[n * SSIZE + n] = pInit[n];
    // 初始化群组跟踪离散矩阵
    memset(inst->gD, 0, sizeof(inst->gD));
    // 初始化G，sFactor
    inst->G = inst->gatingParams->gain;
    inst->sFactor = 1.0f;

    //修改 初始化评分系统
    track_init(&(inst->track_state));
}

void gtrack_unitStop(void* handle)
{
    GtrackUnitInstance* inst;
    inst = (GtrackUnitInstance*)handle;
}

void gtrack_unitPredict(void* handle)
{
    GtrackUnitInstance* inst;

    float temp1[SSIZE * SSIZE];
    float temp2[SSIZE * SSIZE];
    float temp3[SSIZE * SSIZE];

    inst = (GtrackUnitInstance*)handle;
    inst->heartBeatCount++;

    if (inst->isTargetStatic)
    {
        // 对于静态目标，保持S和P
        memcpy(inst->S_apriori_hat, inst->S_hat, sizeof(inst->S_hat));
        memcpy(inst->P_apriori_hat, inst->P_hat, sizeof(inst->P_hat));
    }
    else
    {
        /* obj.S_apriori_hat(1:mSize) = obj.F(1:mSize,1:mSize) * obj.S_hat(1:mSize) */
        gtrack_matrixMultiply(SSIZE, SSIZE, 1, inst->F, inst->S_hat, inst->S_apriori_hat);
        /* obj.P_apriori(1:mSize,1:mSize) = obj.F(1:mSize,1:mSize) * obj.P(1:mSize,1:mSize) * obj.F(1:mSize,1:mSize)' + obj.Q(1:mSize,1:mSize) */
        gtrack_matrixMultiply(SSIZE, SSIZE, SSIZE, inst->F, inst->P_hat, temp1);
        gtrack_matrixTransposeMultiply(SSIZE, SSIZE, SSIZE, temp1, inst->F, temp2);
        gtrack_matrixAdd(SSIZE, SSIZE, temp2, inst->Q, temp3);
        // 矩阵对角化
        gtrack_matrixMakeSymmetrical(SSIZE, temp3, inst->P_apriori_hat);
    }

    // 笛卡尔坐标转换为极坐标，即计算Hs
    gtrack_cartesian2spherical(inst->currentStateVectorType, inst->S_apriori_hat, inst->H_s.array);
    // 计算预测值的约束
    gtrack_calcMeasurementLimits(inst->H_s.vector.range, &inst->gatingParams->limits, &inst->H_limits.vector);
    printf_message(DETAILED_PROCESS_PRINT, "[PREDICT] Trajectory [%d](UID:[%d]) range:[%f],angle:[%f],doopler:[%f]\n", inst->tid, inst->uid, inst->H_s.array[0], inst->H_s.array[1], inst->H_s.array[2]);
    printf_message(DETAILED_PROCESS_PRINT, "          X:[%f],Y:[%f],VX:[%f],VY:[%F]\n", inst->S_apriori_hat[0], inst->S_apriori_hat[1], inst->S_apriori_hat[2], inst->S_apriori_hat[3]);
    printf_message(DEBUG, "          Limits Zone:[r:%f ,a:%f ]\n", inst->H_limits.vector.range, inst->H_limits.vector.angle);
}

void gtrack_unitUpdate(void* handle, GTRACK_measurementPoint* point, float* pBestScore, uint16_t* pAssociate, uint16_t num)
{
    GtrackUnitInstance* inst;

    uint16_t n, idx;
    float    alpha, vel;
    float    J[MSIZE * SSIZE];
    float    PJ[SSIZE * MSIZE];
    float    JPJ[MSIZE * MSIZE];
    float    u_tilda[MSIZE];                                                /* tracker的状态估计值H_s与其关联的所有观测值均值之间的残差 */
    float    cC[MSIZE * MSIZE], cC_inv[MSIZE * MSIZE];   /* 质心的残差协方差矩阵及其逆矩阵 */
    float    K[SSIZE * MSIZE];                                            /* 卡尔曼增益 */
    float    D[MSIZE * MSIZE];                                          /* 离散矩阵 */  
    float    Rm[MSIZE * MSIZE];                                       /* 测量误差协方差矩阵 */
    float    Rc[MSIZE * MSIZE];                                        /* 质心的噪声协方差矩阵，用于Kalman更新 */
    float    temp1[SSIZE * SSIZE];
    bool    targetCanStop = true;

    inst = (GtrackUnitInstance*)handle;

    gtrack_matrixInit(MSIZE, SSIZE, 0.f, J);
    gtrack_matrixInit(MSIZE, MSIZE, 0.f, D);
    gtrack_matrixInit(MSIZE, MSIZE, 0.f, Rm);

    if (num)
    {
        // 更新估计点数
        if (num > inst->estNumOfPoints)
            inst->estNumOfPoints = (float)num;
        else
            inst->estNumOfPoints = 0.99f * inst->estNumOfPoints + 0.01f * (float)num;

        // 估计点数的下限是分配阈值
        if (inst->estNumOfPoints < inst->allocationParams->pointsThre)
            inst->estNumOfPoints = inst->allocationParams->pointsThre;

        // 目标静止，保存状态信息
        if (inst->isTargetStatic)
        {
            memcpy(inst->S_apriori_hat, inst->S_apriori_saved, sizeof(inst->S_apriori_saved));
            inst->isTargetStatic = false;
        }

        inst->center.snr /= num;
        inst->center.doa_snr /= num;
    }
    else
    {
        // 该tracker没有关联点
        if (inst->isTargetStatic == false)
        {
            // 结合任意维度的速度判断目标是否停止运动
            for (n = 0; n < inst->stateVectorDimNum; n++)
            {
                vel = fabsf(inst->S_hat[GTRACK_VEL_DIMENSION * inst->stateVectorDimNum + n]);
                if (vel > GTRACK_MIN_STATIC_VELOCITY)
                {
                    targetCanStop = false;
                    break;
                }
            }
            if (targetCanStop)
            {
                // 目标静止
                inst->isTargetStatic = true;
                // 保存状态信息
                memcpy(inst->S_apriori_saved, inst->S_apriori_hat, sizeof(inst->S_apriori_hat));
                // 强制设置速度及加速度为0
                for (n = 0; n < inst->stateVectorDimNum; n++)
                {
                    inst->S_apriori_hat[GTRACK_VEL_DIMENSION * inst->stateVectorDimNum + n] = 0.f;
                    inst->S_apriori_hat[GTRACK_ACC_DIMENSION * inst->stateVectorDimNum + n] = 0.f;
                }
                // 保持过程协方差矩阵为预测值
                memcpy(inst->P_hat, inst->P_apriori_hat, sizeof(inst->P_hat));
            }
            else
            {
                // 目标没有停止运动，作匀速运动假设
                // 强制设置加速度为0
                for (n = 0; n < inst->stateVectorDimNum; n++)
                {
                    inst->S_apriori_hat[GTRACK_ACC_DIMENSION * inst->stateVectorDimNum + n] = 0.f;
                }
            }
        }
    }

    // 计算测量误差协方差矩阵Rm
    gtrack_unitGetR(inst, inst->center, Rm);

    // 更新群组跟踪离散矩阵gD的估计
    if (num >= GTRACK_MIN_POINTS_TO_UPDATE_DISPERSION)
    {
        // 计算新的离散矩阵D，根据种群中的点与“平均质心”计算的离散程度，并按照一定比例分配给gD
        for (n = 0; n < num; n++)
        {
            idx = pAssociate[n];
            // 计算所有点的协方差
            gtrack_matrixCovAcc(MSIZE, D, point[idx].array, inst->center.array);
        }
        // 归一化D
        gtrack_matrixCovNormalize(MSIZE, D, num);
        // 系数 alpha 从第一次分配时的最大值（1.f）下降到长时间观察目标后的最小值 0.1f。
        alpha = ((float)num) / inst->estNumOfPoints;
        // 计算gD，inst->gD = (1-alpha)*inst->gD + alpha*D
        gtrack_matrixCovFilter(MSIZE, inst->gD, D, alpha);
    }

    // 计算状态向量的偏导（雅各比矩阵）
    gtrack_computeJacobian(inst->currentStateVectorType, inst->S_apriori_hat, J);
    // 计算 JPJ' = J(:,1:mSize) * obj.P_apriori(1:mSize,1:mSize) * J(:,1:mSize)'
    gtrack_matrixComputePJT(inst->P_apriori_hat, J, PJ);
    gtrack_matrixMultiply(MSIZE, SSIZE, MSIZE, J, PJ, JPJ);

    if (num)
    {
        // 根据当前目标状态展开质心径向速度
        gtrack_velocityStateHandling(inst, &(inst->center.vector));
        /* 计算质心的测量噪声协方差矩阵Rc，用于Kalman更新 */
        /* 第一项表示对测量的质心的误差，随着测量次数的增加而减小 */
        /* 第二项表示由于并非所有成员都观测到了而导致的质心而不确定性。*/
        /* Rc = Rm + alpha * unit.gD; */

        /* alpha 是加权因子，是关联点数与群组内点数的函数关系 */
        /* alpha = (unit.maxAssociatedPoints-num)/((unit.maxAssociatedPoints-1)*num); */
        alpha = ((float)(inst->estNumOfPoints - num)) / ((inst->estNumOfPoints - 1) * num);
        // 计算Rc，取对角元素
        gtrack_uintGetRc(Rm, inst->gD, alpha, Rc);
        // 计算tracker的状态估计值H_s与其关联的所有观测值均值之间的残差 innovation
        gtrack_matrixSub(MSIZE, 1, inst->center.array, inst->H_s.array, u_tilda);
        // 计算质心残差的协方差矩阵 cC = [3x6]x[6x6]x[6x3]+[3x3]
        // cC = J(:,1:mSize) * obj.P_apriori(1:mSize,1:mSize) * J(:,1:mSize)' + Rc
        gtrack_matrixAdd(MSIZE, MSIZE, JPJ, Rc, cC);
        // 计算质心残差的协方差矩阵的逆
        gtrack_matrixInv(cC, temp1, cC_inv);
        // 计算kalman增益 K[6x3] = P[6x6]xJ[3x6]'xIC_inv[3x3]=[6x3]
        // K = obj.P_apriori(1:mSize,1:mSize) * J(:,1:mSize)' * iC_inv
        gtrack_matrixMultiply(SSIZE, MSIZE, MSIZE, PJ, cC_inv, K);
        // 状态估计 obj.S_hat(1:mSize) = obj.S_apriori_hat(1:mSize) + K * u_tilda
        gtrack_uintGetSHat(K, u_tilda, inst->S_apriori_hat, inst->S_hat);
        // 误差协方差估计
        // obj.P(1:mSize,1:mSize) = obj.P_apriori(1:mSize,1:mSize) - K * J(:,1:mSize) * obj.P_apriori(1:mSize,1:mSize)
        gtrack_uintGetPHat(K, PJ, inst->P_apriori_hat, inst->P_hat);
    }
    else
    {
        /* Handling of erasures */
        memcpy(inst->S_hat, inst->S_apriori_hat, sizeof(inst->S_hat));
        memcpy(inst->P_hat, inst->P_apriori_hat, sizeof(inst->P_hat));
    }

    // gd表示簇内每个点的与重心误差协方差矩阵（群体的分散性），描述点差异情况；rm表示簇内点与簇空间中心的协方差矩阵
    // RC作为测量误差矩阵取上述矩阵对角加权平均
    // 计算群组协方差 gC = Rm +  JPJ  + gD
    // 使用椭圆作为门限，以考虑到群体的分散性、目标机动性和测量噪声。
    gtrack_uintGetgC(JPJ, Rm, inst->gD, inst->gC);

    // 计算群组协方差gC的逆
    gtrack_matrixInv(inst->gC, &inst->gC_det, inst->gC_inv);
    printf_message(DETAILED_PROCESS_PRINT, "[UPDATE] Trajectory [%d](UID:[%d]) X:[%f],Y:[%f],VX:[%f],VY:[%F],XA:[%f],YA:[%f]\n", inst->tid, inst->uid, inst->S_hat[0], inst->S_hat[1], inst->S_hat[2], inst->S_hat[3],inst->S_hat[4],inst->S_hat[5]);
    
    // 更新状态机
    gtrack_unitConfidence(inst, num);// 更新tracker的得分
}

void gtrack_unitScore(void* handle, GTRACK_measurementPoint* point, uint16_t* allocIndex, float* bestScore, uint8_t* bestInd, GTRACK_measurementPoint center, uint16_t num)
{
    GtrackUnitInstance* inst = (GtrackUnitInstance*)handle;
    GTRACK_measurementUnion u_tilda;
    float rvOut, mdp, md, score;
    //float logdet;
    bool  isWithinLimits;
    uint16_t m, idx;
    // 计算测量值与预测值的残差
    gtrack_vectorSub(MSIZE, center.array, inst->H_s.array, u_tilda.array);
    if (inst->velocityHandling < VELOCITY_LOCKED)
    {
        // 径向速度展开（类似速度解模糊）,根据群组质心的径向速度
        rvOut = gtrack_unrollRadialVelocity(inst->radarParams->maxRadialVelocity, inst->allocationVelocity, center.vector.doppler);
        u_tilda.vector.doppler = rvOut - inst->allocationVelocity;
    }
    else
    {
        // 径向速度展开（类似速度解模糊）,根据预测的径向速度
        rvOut = gtrack_unrollRadialVelocity(inst->radarParams->maxRadialVelocity, inst->H_s.vector.doppler, center.vector.doppler);
        u_tilda.vector.doppler = rvOut - inst->H_s.vector.doppler;
    }
    center.vector.doppler = rvOut;

    // 若当前点聚类中心与预测的残差超过当前预测值的约束条件，则舍弃之
    isWithinLimits = true;
    for (m = 0; m < MSIZE; m++)
    {
        if (fabs(u_tilda.array[m]) > inst->H_limits.array[m])
        {
            isWithinLimits = false;
            break;
        }
    }

     if (isWithinLimits == false)
        return;

    // 计算当前聚类中心到当前tracker群组质心的距离，忽略doppler（为什么不计算doppler?）
    // d = y'* inv(Cg) * y
    gtrack_computeMahalanobisPartial(u_tilda.array, inst->gC_inv, &mdp);
    // 一个待观测值能被关联到该轨迹的条件是：当前测量值添加到已有轨迹时的 innovation 的大小mdp小于常数阈值G
    printf_message(DEBUG, "[Associate] Trajectory [%d](UID:[%d]) mdp :[%f - %f]\n", (uint8_t)inst->tid, (uint8_t)inst->uid, mdp, inst->G);

    if (mdp < inst->G)
    {
        // 打分，score = d + log|Ci|
        gtrack_computeMahalanobis(u_tilda.array, inst->gC_inv, &md);
        //logdet = logf(inst->gC_det);
        //score = logdet + md;
        score = md;

        for (m = 0; m < num; m++)
        {
            idx = allocIndex[m];
            // 更新最佳分数
            if (score < bestScore[idx])
            {
                bestScore[idx] = score;
                bestInd[idx] = (uint8_t)inst->uid;
                point[idx].vector.doppler = gtrack_unrollRadialVelocity(inst->radarParams->maxRadialVelocity, center.vector.doppler, point[idx].vector.doppler);
            }
        }
    }
}

void gtrack_unitGetR(void* handle, GTRACK_measurementPoint center, float* R)
{
    GtrackUnitInstance* inst = (GtrackUnitInstance*)handle;
    GTRACK_RadarParams* radarParams = inst->radarParams;

    // 除SNR为变量外，其余均为常量，可预先计算，无需每次计算
    float init_range = (radarParams->rangeResolution) * (radarParams->rangeResolution) / 4.f;
    float init_angle = radarParams->radResolution * radarParams->radResolution / 4.f;
    float init_doppler = radarParams->radialVelocityResolution * radarParams->radialVelocityResolution / 4.f;

    GTRACK_measurementUnion var;
    memset(&var, 0, sizeof(GTRACK_measurementUnion));
    // 距离噪声：v_r^2 = rangeResolution^2 / (4 * SNR)
    var.vector.range = init_range / center.snr;
    // 角度噪声：v_theta^2 = radResolution^2 / (4 * SNR)
    var.vector.angle = init_angle / center.doa_snr;
    // 速度噪声：v_doppler^2 = radialVelocityResolution^2 / (4 * SNR)
    var.vector.doppler = init_doppler / center.snr;

    // 假设各维度噪声相互独立，则观测噪声矩阵R为对角阵
    for (int i = 0; i < MSIZE; i++)
    {
        R[i + i * MSIZE] = var.array[i];
    }
}

void gtrack_uintGetSHat(float* K, float* y, float* S_apriori, float* S_hat)
{
    /* S_hat(m*1) = K(m*n) * y(n*1) + S_apriori(m*1) */
    uint16_t i, j;
    float temp;
    for (i = 0; i < SSIZE; i++)
    {
        temp = 0.f;
        for (j = 0; j < MSIZE; j++)
        {
            temp += K[i * MSIZE + j] * y[j];
        }
        S_hat[i] = temp + S_apriori[i];
    }
}

void gtrack_uintGetPHat(float* K, float* PJ, float* P_apriori, float* P_hat)
{
    /* P_hat(m*m) = P_apriori(m*m) - K(m*n) * PJ(m*n)^T */
    uint16_t i, j, k;
    float temp;
    for (i = 0; i < SSIZE; i++)
    {
        for (j = 0; j < SSIZE; j++)
        {
            temp = 0.f;
            for (k = 0; k < MSIZE; k++)
            {
                temp += K[i * MSIZE + k] * PJ[k + j * MSIZE];
            }
            P_hat[i * SSIZE + j] = P_apriori[i * SSIZE + j] - temp;
        }
    }
}

void gtrack_uintGetRc(float* Rm, float* gD, float alpha, float* Rc)
{
    memset(Rc, 0, sizeof(float) * MSIZE * MSIZE);
    // 假设各维度数据相互独立，Rc为对角阵
    // Rc = Rm + alpha * gD;
    int i, j;
    for (i = 0; i < MSIZE; i++)
    {
        j = i * MSIZE + i;
        Rc[j] = Rm[j] + alpha * gD[j];
    }
}

void gtrack_uintGetgC(float* JPJ, float* Rm, float* gD, float* gC)
{
    uint16_t i;
    for (i = 0U; i < MSIZE * MSIZE; i++)
    {
        gC[i] = JPJ[i] + Rm[i] + gD[i];
    }
}

void gtrack_velocityStateHandling(void* handle, GTRACK_measurement_vector* uVec)
{
    GtrackUnitInstance* inst;
    float               instanteneousRangeRate;
    float               rrError;
    float               rvError;
    float               rvIn;

    inst = (GtrackUnitInstance*)handle;
    rvIn = uVec->doppler;

    switch (inst->velocityHandling)
    {

    case VELOCITY_INIT:
        // 初始状态下doppler直接等于距离变化率
        uVec->doppler = inst->rangeRate;
        inst->velocityHandling = VELOCITY_RATE_FILTER;
        break;
    case VELOCITY_RATE_FILTER:
        // 本状态下使用距离变化率展开径向速度，稳定距离变化率
        // 计算瞬时距离变化率
        instanteneousRangeRate = (uVec->range - inst->allocationRange) / ((inst->heartBeatCount - inst->allocationTime) * inst->radarParams->deltaT);
        // 更新距离变化率
        inst->rangeRate = inst->unrollingParams->alpha * inst->rangeRate + (1 - inst->unrollingParams->alpha) * instanteneousRangeRate;
        // 径向速度展开
        uVec->doppler = gtrack_unrollRadialVelocity(inst->radarParams->maxRadialVelocity, inst->rangeRate, rvIn);
        // 计算距离变化率误差
        rrError = (instanteneousRangeRate - inst->rangeRate) / inst->rangeRate;
        // 距离变化率误差小于展开置信度时切换速度处理状态
        if (fabsf(rrError) < inst->unrollingParams->confidence)
        {
            inst->velocityHandling = VELOCITY_TRACKING;
        }
        break;
    case VELOCITY_TRACKING:
        // 本状态下使用距离变化率展开径向速度，同时监测Hs的预测速度误差
        // 计算瞬时距离变化率
        instanteneousRangeRate = (uVec->range - inst->allocationRange) / ((inst->heartBeatCount - inst->allocationTime) * inst->radarParams->deltaT);
        // 更新距离变化率，径向速度展开
        inst->rangeRate = inst->unrollingParams->alpha * inst->rangeRate + (1 - inst->unrollingParams->alpha) * instanteneousRangeRate;
        uVec->doppler = gtrack_unrollRadialVelocity(inst->radarParams->maxRadialVelocity, inst->rangeRate, rvIn);
        // 计算Hs速度误差，误差小于0.1时切换速度处理状态
        rvError = (inst->H_s.vector.doppler - uVec->doppler) / uVec->doppler;
        if (fabsf(rvError) < 0.1f)
        {
            inst->velocityHandling = VELOCITY_LOCKED;
        }
        break;
    case VELOCITY_LOCKED:
        // 本状态下使用Hs的预测速度展开径向速度
        uVec->doppler = gtrack_unrollRadialVelocity(inst->radarParams->maxRadialVelocity, inst->H_s.vector.doppler, uVec->doppler);
        break;
    }
}